#include "Claw.h"
#include "ARM.h""
#include "controller.h"
#include "DJI_motor.h"

#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

#include "stdio.h"
#include "DJI_motor.h"
#include "mainc.h"

PIDInstance CLAW1[2];
PIDInstance CLAW2[2];
PIDInstance CLAW3[2];
PIDInstance CLAW4[2];
PIDInstance CLAW5[2];



float Claw_out[5][2];
PID_Init_Config_s CLAW1_pid[2] = {
    {.Kp = 40,
     .Ki = 15,
     .Kd = 0.1,
     .MaxOut = 5000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 50},
    {.Kp = 8,
     .Ki = 0.01,
     .Kd = 0.01,
     .MaxOut = 10000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 1000}};
PID_Init_Config_s CLAW1_pid_1[2] = {
    {.Kp = 2,
     .Ki = 0,
     .Kd = 0.1,
     .MaxOut = 1000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 50},
    {.Kp = 8,
     .Ki = 0.01,
     .Kd = 0.01,
     .MaxOut = 10000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 1000}};
PID_Init_Config_s CLAW2_pid[2] = {
    {.Kp = 40,
     .Ki = 15,
     .Kd = 0.1,
     .MaxOut = 5000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 50},
    {.Kp = 8,
     .Ki = 0.01,
     .Kd = 0.01,
     .MaxOut = 10000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 1000}};
PID_Init_Config_s CLAW2_pid_2[2] = {
    {.Kp = 2,
     .Ki = 0,
     .Kd = 0.1,
     .MaxOut = 1000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 50},
    {.Kp = 8,
     .Ki = 0.01,
     .Kd = 0.01,
     .MaxOut = 10000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 1000}};
PID_Init_Config_s CLAW3_pid[2] = {
    {.Kp = 40,
     .Ki = 15,
     .Kd = 0.1,
     .MaxOut = 5000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 50},
    {.Kp = 8,
     .Ki = 0.01,
     .Kd = 0.01,
     .MaxOut = 10000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 1000}};
PID_Init_Config_s CLAW3_pid_3[2] = {
    {.Kp = 2,
     .Ki = 0,
     .Kd = 0.1,
     .MaxOut = 1000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 50},
    {.Kp = 8,
     .Ki = 0.01,
     .Kd = 0.01,
     .MaxOut = 10000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 1000}};
PID_Init_Config_s CLAW4_pid[2] = {
    {.Kp = 40,
     .Ki = 15,
     .Kd = 0.1,
     .MaxOut = 5000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 50},
    {.Kp = 8,
     .Ki = 0.01,
     .Kd = 0.01,
     .MaxOut = 10000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 1000}};
PID_Init_Config_s CLAW4_pid_4[2] = {
    {.Kp = 2,
     .Ki = 0,
     .Kd = 0.1,
     .MaxOut = 1000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 50},
    {.Kp = 8,
     .Ki = 0.01,
     .Kd = 0.01,
     .MaxOut = 10000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 1000}};
PID_Init_Config_s CLAW5_pid[2] = {
    {.Kp = 40,
     .Ki = 15,
     .Kd = 0.1,
     .MaxOut = 5000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 50},
    {.Kp = 8,
     .Ki = 0.01,
     .Kd = 0.01,
     .MaxOut = 10000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 1000}};
PID_Init_Config_s CLAW5_pid_5[2] = {
    {.Kp = 2,
     .Ki = 0,
     .Kd = 0.1,
     .MaxOut = 1000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 50},
    {.Kp = 8,
     .Ki = 0.01,
     .Kd = 0.01,
     .MaxOut = 10000,
     .Improve = PID_Integral_Limit,
     .IntegralLimit = 1000}};
//int32_t up_pos = 4898;
int32_t first_up = 1000;
int32_t up_pos = 4848;
int32_t set_pos = 3893;
//int32_t set_pos = 1000;
int32_t up_pos_5_1 = 100 ;
int32_t up_pos_5 = 3050 ;
int32_t set_pos_5 = 2000 ;
int16_t claw_tim = 0;
uint8_t or_sencod[5] = {0};
uint16_t claw_delay = 25;  // 第一次抓取延时
uint16_t claw_delay2 = 4 * 250; // 第一次抓取延时

void CawPIDInit(void);
void CawPIDInit_1(void);
void Moto_Zero_angle(void);
void Claw_clc(void);
void Claw(void *argument)
{
    // CawPIDInit();
    osDelay(1000);
    CawPIDInit();
    Moto_Zero_angle(); // 380000放置 450000抬高
    uint32_t current_time;

    for (;;)
    {
        current_time = xTaskGetTickCount();
        if (CMode == 0)
        {
            Moto_Zero_angle();
        }
				else if (CMode == 2)
				{	
					if((RxData[1]&0x01)!=0x01)
						Claw_Motor[0].target_angle = first_up;
					if((RxData[1]&0x02)!=0x02)
						Claw_Motor[1].target_angle = first_up;
					if((RxData[1]&0x04)!=0x04)
						Claw_Motor[2].target_angle = first_up;
					if((RxData[1]&0x08)!=0x08)
						Claw_Motor[3].target_angle = first_up;
					
					if(Down[0])
						Claw_Motor[0].target_angle = 0;
					if(Down[1])
						Claw_Motor[1].target_angle = 0;
					if(Down[2])
						Claw_Motor[2].target_angle = 0;
					if(Down[3])
						Claw_Motor[3].target_angle = 0;
					
					//Claw_Motor[3].target_angle = up_pos;
					//Claw_Motor[2].target_angle = up_pos;
					Claw_Motor[4].target_angle = up_pos_5_1;
					
				}
        else if (CMode == 3)
        {
            Claw_Motor[0].target_angle = up_pos;
            Claw_Motor[1].target_angle = up_pos;
            Claw_Motor[2].target_angle = up_pos;
            Claw_Motor[3].target_angle = up_pos;
            Claw_Motor[4].target_angle = up_pos_5;
            if (claw_delay == 0)
            {
//                for (uint8_t i = 0; i < 5; i++)
//                {
//                    if (Claw_out[i][1] < 800 && Claw_out[i][1] > -800) // 抓取不到?
//                    {
//                        or_sencod[i] = 1;
//                    }
//                }
                CMode++;
            }
            else
            {
                claw_delay--;
            }
        }
//        else if (CMode == 4)
//        {
//            for (uint8_t i = 0; i < 5; i++)
//            {
//                if (or_sencod[i] == 1)
//                    Claw_Motor[i].target_angle = 0;
//            }
//        }
//        else if (CMode == 5)
//        {
//            Claw_Motor[0].target_angle = up_pos;
//            Claw_Motor[1].target_angle = up_pos;
//            Claw_Motor[2].target_angle = up_pos;
//            Claw_Motor[3].target_angle = up_pos;
//            Claw_Motor[4].target_angle = up_pos;
//            if (claw_delay2 == 0)
//            {
//                CMode++;
//            }
//            else
//            {
//                claw_delay2--;
//            }
//        }
        else if (CMode == 5)	//8
        {		
						CLAW1[0].MaxOut=1500;
						CLAW2[0].MaxOut=1500;
					  CLAW3[0].MaxOut=1500;
						CLAW4[0].MaxOut=1500;
						CLAW5[0].MaxOut=1500;
						
						//CawPIDInit_1();
            Claw_Motor[1].target_angle = set_pos;
            Claw_Motor[2].target_angle = set_pos;	
						Claw_Motor[4].target_angle = set_pos_5;	
            int32_t err1, err2;
            err1 = Claw_Motor[1].target_angle - Claw_Motor[1].totall_angle;
            err2 = Claw_Motor[2].target_angle - Claw_Motor[2].totall_angle;
            if (err2 < 10 && err1 < 10 && err1 > -10 && err2 > -10)
            {
                claw_tim++;
                if (claw_tim > 5)
                {
                    CMode++;
                    claw_tim = 0;
                }
            }
        }
        else if (CMode == 8)	//12
        {
            Claw_Motor[0].target_angle = set_pos;
            Claw_Motor[3].target_angle = set_pos;
            int32_t err1, err2;
            err1 = Claw_Motor[0].target_angle - Claw_Motor[0].totall_angle;
            err2 = Claw_Motor[3].target_angle - Claw_Motor[3].totall_angle;
            if (err2 < 10 && err1 < 10 && err1 > -10 && err2 > -10)
            {
                claw_tim++;
                if (claw_tim > 5)
                {
                    CMode++;
                    claw_tim = 0;
                }
            }
        }
        if (CMode != 0)
        {
            Claw_clc();
						
            Claw_Motor[0].tx_data = Claw_out[0][1];
            Claw_Motor[1].tx_data = Claw_out[1][1];
            Claw_Motor[2].tx_data = Claw_out[2][1];
            Claw_Motor[3].tx_data = Claw_out[3][1];
            Claw_Motor[4].tx_data = Claw_out[4][1];
        }
        else
        {
            Claw_Motor[0].tx_data = 0;
            Claw_Motor[1].tx_data = 0;
            Claw_Motor[2].tx_data = 0;
            Claw_Motor[3].tx_data = 0;
            Claw_Motor[4].tx_data = 0;
        }
        Claw_Send();
        vTaskDelayUntil(&current_time, 4); // 绝对延时
    }
}

void Moto_Zero_angle(void)
{
    Claw_Motor[0].offset_angle = Claw_Motor[0].angle;
    Claw_Motor[0].round = 0;
    Claw_Motor[0].totall_angle = 0;

    Claw_Motor[1].offset_angle = Claw_Motor[1].angle;
    Claw_Motor[1].round = 0;
    Claw_Motor[1].totall_angle = 0;

    Claw_Motor[2].offset_angle = Claw_Motor[2].angle;
    Claw_Motor[2].round = 0;
    Claw_Motor[2].totall_angle = 0;

    Claw_Motor[3].offset_angle = Claw_Motor[3].angle;
    Claw_Motor[3].round = 0;
    Claw_Motor[3].totall_angle = 0;
	
		Claw_Motor[4].offset_angle = Claw_Motor[4].angle;
    Claw_Motor[4].round = 0;
    Claw_Motor[4].totall_angle = 0;
}

void CawPIDInit(void)
{
    PIDInit(&CLAW1[0], &CLAW1_pid[0]);
    PIDInit(&CLAW1[1], &CLAW1_pid[1]);

    PIDInit(&CLAW2[0], &CLAW2_pid[0]);
    PIDInit(&CLAW2[1], &CLAW2_pid[1]);

    PIDInit(&CLAW3[0], &CLAW3_pid[0]);
    PIDInit(&CLAW3[1], &CLAW3_pid[1]);

    PIDInit(&CLAW4[0], &CLAW4_pid[0]);
    PIDInit(&CLAW4[1], &CLAW4_pid[1]);

    PIDInit(&CLAW5[0], &CLAW5_pid[0]);
    PIDInit(&CLAW5[1], &CLAW5_pid[1]);
}

void CawPIDInit_1(void)
{
    PIDInit(&CLAW1[0], &CLAW1_pid_1[0]);
    PIDInit(&CLAW1[1], &CLAW1_pid_1[1]);

    PIDInit(&CLAW2[0], &CLAW2_pid_2[0]);
    PIDInit(&CLAW2[1], &CLAW2_pid_2[1]);

    PIDInit(&CLAW3[0], &CLAW3_pid_3[0]);
    PIDInit(&CLAW3[1], &CLAW3_pid_3[1]);

    PIDInit(&CLAW4[0], &CLAW4_pid_4[0]);
    PIDInit(&CLAW4[1], &CLAW4_pid_4[1]);

    PIDInit(&CLAW5[0], &CLAW5_pid_5[0]);
    PIDInit(&CLAW5[1], &CLAW5_pid_5[1]);
}

void Claw_clc(void)
{
    Claw_out[0][0] = PIDCalculate(&CLAW1[0], Claw_Motor[0].totall_angle, Claw_Motor[0].target_angle); // 位置环
    Claw_out[0][1] = PIDCalculate(&CLAW1[1], Claw_Motor[0].rpm, Claw_out[0][0]);

    Claw_out[1][0] = PIDCalculate(&CLAW2[0], Claw_Motor[1].totall_angle, Claw_Motor[1].target_angle); // 位置环
    Claw_out[1][1] = PIDCalculate(&CLAW2[1], Claw_Motor[1].rpm, Claw_out[1][0]);

    Claw_out[2][0] = PIDCalculate(&CLAW3[0], Claw_Motor[2].totall_angle, Claw_Motor[2].target_angle); // 位置环
    Claw_out[2][1] = PIDCalculate(&CLAW3[1], Claw_Motor[2].rpm, Claw_out[2][0]);

    Claw_out[3][0] = PIDCalculate(&CLAW4[0], Claw_Motor[3].totall_angle, Claw_Motor[3].target_angle); // 位置环
    Claw_out[3][1] = PIDCalculate(&CLAW4[1], Claw_Motor[3].rpm, Claw_out[3][0]);
		
		Claw_out[4][0] = PIDCalculate(&CLAW5[0], Claw_Motor[4].totall_angle, Claw_Motor[4].target_angle); // 位置环
    Claw_out[4][1] = PIDCalculate(&CLAW5[1], Claw_Motor[4].rpm, Claw_out[4][0]);
}


